
#include "sku622.h"
#include <string.h>

/* Byte 0~3 are message headers. */
#define SKU622_COM_BYTE_0   (0xDD)
#define SKU622_COM_BYTE_1   (0x66)
#define SKU622_COM_BYTE_2   (0x0A)
#define SKU622_COM_BYTE_3   (0x00)
/* Byte 4~5 are the device ID. */

/* Byte 6~7 are tested device ID. */

/* Byte 8~11 are measuring distance. */

/* Byte 12~13 are message tail. */
#define SKU622_COM_BYTE_12  (0xBB)
#define SKU622_COM_BYTE_13  (0xAA)



SKU622_TypeDef sku622_Data;


/**
 * @function:SKU622_UnpackBuffer
 * @brief
 * @param {uint8_t} *DataBuff: buffer
 * @param {uint16_t} DataLen: lenght
 * @return {uint8_t: 0: OK. Other: Error}
 */
uint8_t SKU622_UnpackBuffer(uint8_t *DataBuff, uint16_t DataLen)
{
    static uint8_t TempBuff[128] = {0};
    static uint16_t DataCnt = 0;
    uint16_t HeadPoint = 0;

    if ((sizeof(TempBuff) - DataCnt) > DataLen)
    {
        memcpy(TempBuff + DataCnt, DataBuff, DataLen);
    }
    else
    {
        DataCnt = 0;
        memcpy(TempBuff, DataBuff, DataLen);
    }
    DataCnt += DataLen;

    /* Check lenght. */
    if (DataCnt < 14)
    {
        return 1;
    }

    /* Find message headers. */
    for (uint8_t i = 0; i < DataCnt - 13; i++)
    {
        if (SKU622_COM_BYTE_0 == TempBuff[i])
        {
            HeadPoint = i;
            break;
        }
    }
    if (DataCnt - HeadPoint < 14)
    {
        return 2;
    }

    /* Check message. */
    if (TempBuff[HeadPoint + 0] != SKU622_COM_BYTE_0 || TempBuff[HeadPoint + 1] != SKU622_COM_BYTE_1 || \
        TempBuff[HeadPoint + 2] != SKU622_COM_BYTE_2 || TempBuff[HeadPoint + 3] != SKU622_COM_BYTE_3 || \
        TempBuff[HeadPoint + 12] != SKU622_COM_BYTE_12 || TempBuff[HeadPoint + 13] != SKU622_COM_BYTE_13)
    {
        return 3;
    }
    /* Get ID */
    sku622_Data.Self_id = TempBuff[HeadPoint + 4] << 8 | TempBuff[HeadPoint + 5];
    sku622_Data.Measure_id = TempBuff[HeadPoint + 6] << 8 | TempBuff[HeadPoint + 7];
    /* Distance data. */
    sku622_Data.Distance = TempBuff[HeadPoint + 11] << 24 | TempBuff[HeadPoint + 10] << 16 | TempBuff[HeadPoint + 9] << 8 | TempBuff[HeadPoint + 8];
    sku622_Data.UpDataFlag = 1;

    /* Organize data. */
    memcpy(TempBuff, TempBuff + HeadPoint + 14, DataLen);
    DataCnt = DataCnt - HeadPoint - 14;

    return 0;
}

/**
 * @function: SKU622_Init
 * @brief
 * @return {*}
 */
void SKU622_Init(void)
{

}

/**
 * @function: SKU622_GetDis
 * @brief Get UWB measurement data
 * @return {uint32_t: Distance}
 */
uint32_t SKU622_GetDis(void)
{
    return sku622_Data.Distance;
}

















